
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <ros/time.h>
#include "can-ctrl.h"
#include "zeus_s2_driver/controlcan.h"
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
// #include "dirve_ros-master/dirve_ctr.h"
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <dynamic_reconfigure/server.h>
#include <tf/tf.h>
#include "sensor_msgs/JointState.h"
#include "zeus_s2_msgs/Feedback.h"
#include "zeus_s2_msgs/wheel_state.h"
#include "zeus_s2_msgs/motor_pid_vol.h"
#include "zeus_s2_msgs/joy_control.h"
// #include <zeus_s2_driver/uart-ctrl.h>
#include <zeus_s2_driver/PID_reconfigConfig.h> 
// #include "zeus_s2_driver/my_timer.h"

/*  使用高配版nvidia-xavier主机，自带can通信*/
#define NVIDIA_XAVIER    1

#define MAX_LINEAR  1.57        //m/s
#define MAX_ANGULAR  0.7853     //red/s

#define GROUP_INT_BYTE(X,N)   ((X[N+3]<<24)+(X[N+2]<<16)+(X[N+1]<<8)+X[N])
#define  MOTOR_ID(X,N)   ((X[N]<<8 | X[N+1])>>5)

#define PID_INTEGRAL_UP    0.8   //积分上限
#define PID_SCALE      1.0     //PID缩放系数
#define PID_TEMP        1.5

#define MAX      1910.0        //32767
#define MIN      -1910.0        //-32768

#define PI                        3.1415926

enum _enum_DirveID{

  DRIVER_OPERATION_MODE = 1,
  DRIVER_TIMEOUT_DETECTION = 2,
  DRIVER_PARKING_MODE = 3,
  DRIVER_SWITCHING_MODE = 0,

	DIRVEE_ID_SYSTEM_STATE_ACK = 0x151,
	DIRVEE_ID_MOVE_CONTROL_ACK = 0x131,
	DIRVEE_ID_MOVE_CONTROL = 0x130,
	DIRVEE_ID_ONE_MOTOR_ACK = 0x201,
	DIRVEE_ID_TWO_MOTOR_ACK = 0x202,
	DIRVEE_ID_THREE_MOTOR_ACK = 0x203,
	DIRVEE_ID_FOUR_MOTOR_ACK = 0x204,
	DIRVEE_ID_LAMPLIGHT_CONTROL = 0x140,		//lamplight
	DIRVEE_ID_LAMPLIGHT_CONTROL_ACK = 0x141,

   DRIVER_ID_ONE_POSE_ACK = 0X601,
   DRIVER_ID_TWO_POSE_ACK = 0X602,
   DRIVER_ID_THREE_POSE_ACK = 0X603,
   DRIVER_ID_FOUR_POSE_ACK = 0X604,

   DRIVER_ID_ONE_CONTROL = 0x101,
   DRIVER_ID_TWO_CONTROL = 0x102,
   DRIVER_ID_THREE_CONTROL = 0x103,
   DRIVER_ID_FOUR_CONTROL = 0x104,

  DRIVER_ID_MODE_ACK = 0X700,



 
};

enum  _enum_timeManagement{

  TIME_DRIVER_MODE=0,
   TIME_SUM,
 
};

struct  DIRVEDATA
{
   uint8_t carBodyStateAck;
   int8_t  modeControlAck;
   uint16_t batteryVoltageAck;
   uint16_t faultMessageAck;

   int16_t movementSpeedAck;
   int16_t rotationalSpeedAck;
	
   uint8_t controlMode;
   uint8_t faultClearance;
   int8_t  percentLinear;
   int8_t  percentAngular;
   
   uint16_t oneMotorElectricCurrentAck;
   int16_t oneMotorRorateSpeedAck;
   int8_t  oneMotorTemperatureAck;
   
   uint16_t twoMotorElectricCurrentAck;
   int16_t twoMotorRorateSpeedAck;
   int8_t  twoMotorTemperatureAck;

   uint16_t threeMotorElectricCurrentAck;
   int16_t threeMotorRorateSpeedAck;
   int8_t  threeMotorTemperatureAck;

   uint16_t fourMotorElectricCurrentAck;
   int16_t fourMotorRorateSpeedAck;
   int8_t  fourMotorTemperatureAck;

   uint8_t lightControlEnableSign;
   uint8_t bodyLightPattern;

   uint8_t currentLightControlEnableSignAck;
   uint8_t bodyLightPatternAck;
} ;

class DriverPub
{
   public:
        DriverPub();
        ~DriverPub();
        void loop();
    private:
        bool initRobot();
        void receive_func();
        void send_speed_callback(void);
        void handle_speed_data(uint16_t number);
        void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
        void cmd_joy_callback(const zeus_s2_msgs::joy_control::ConstPtr& msg);
        // uint8_t get_line_speed(void);
        // uint8_t get_angular_speed(void);
        void enable_motor_driver(uint16_t driver_id );
        void disable_motor_driver(uint16_t driver_id );
        void clear_ALM_motor_driver(uint16_t driver_id );
        void Set_motor_torque_mode(uint16_t driver_id );
        void Set_torque_value_to_motor(uint16_t driver_id ,uint16_t value );
        // void setSendData_speed(uint8_t line_speed,uint8_t angular_speed);
        // bool check_sum(VCI_CAN_OBJ * rec_buff);
#ifdef  NVIDIA_XAVIER
         int step=0;
	      char tx_buf[8] = {0, 0x1E, 0, 0x03, 0, 1, 0, 0};
	      char rx_buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
        struct can_frame can_id;
        void Calculate_LeftAndRight_Meter(struct can_frame *p);
        bool mode_switch_judge(struct can_frame  *p,uint8_t mode,bool isnot);
#else

        VCI_BOARD_INFO pInfo;//用来获取设备信息。
        VCI_BOARD_INFO pInfo1 [50];
        void dirve_ProcessData(VCI_CAN_OBJ *p,DIRVEDATA *PdirveDate );
        void Calculate_LeftAndRight_Meter(VCI_CAN_OBJ *p);
        bool mode_switch_judge(VCI_CAN_OBJ *p,uint8_t mode,bool isnot);

#endif
      
        uint8_t get_recvFlag(void);
        // int32_t get_rev_leftVol(void);
        // int32_t get_rev_rightVol(void);
        // void set_walkingmotor_speed_angle(double line_speed,double angle_speed);
      //   void read_motor_position(int fdcom,uint16_t number);
      //   void set_motor_bg(int fdcom);
      //   void send_motor_current(int fdcom,uint16_t number,int16_t currentVol);
        int16_t  motor_velocityPidCtl_one(float target_speed,float current_speed);
        int16_t  motor_velocityPidCtl_two(float target_speed,float current_speed);
       int16_t  motor_velocityPidCtl_three(float target_speed,float current_speed);
        int16_t  motor_velocityPidCtl_four(float target_speed,float current_speed);
      //   void set_can_command_mode(int fdcom);
      //   void set_motor_mo(int fdcom,uint16_t number);
      //   void set_motor_um(int fdcom,uint16_t number);
        void dynamic_reconfig_callback(zeus_s2_driver::PID_reconfigConfig &config,uint32_t level);
        void motor_driver_check(void);
        void Set_motor_speed_mode(uint16_t driver_id );
        void Set_speed_value_to_motor(uint16_t driver_id ,uint16_t value );
        uint64_t my_xTaskGetTickCount(void);
        void  time_setMana(unsigned char time_id);
        unsigned char time_checkHandle(unsigned char time_id, unsigned short time_ms);
      //   void motor_driver_clearFlag(uint16_t number);
      //    void motor_driver_recFlag(uint16_t number);
     
      void mode_switching_state_machine(uint8_t state, bool isnot);
      
        boost::mutex cmd_vel_mutex_;
        ros::Time last_twist_time_;
        
        boost::mutex mutex_;

        ros::Subscriber cmd_sub_;
         ros::Subscriber cmd_joy_moment_;
        ros::Publisher odom_pub_;
        ros::Publisher wheel_state_feedback_;
        zeus_s2_msgs::Feedback  left_right_wheel_state_;
        ros::Publisher speed_pid_;              //发布测试pid话题
        zeus_s2_msgs::motor_pid_vol  motor_pid_[4];      //调试pid参数类型

        ros::Timer timer1;                                  //ros的定时器
      //   ros::Publisher front_right_wheel_state_;
      //   os::Publisher rear_left_wheel_state_;
      //   ros::Publisher rear_right_wheel_state_;

        std::string code_version_;
        std::string port_name_;

        std::string odom_frame_;
        std::string base_frame_;

        bool recv_flag_;
        bool start_flag_;
        bool publish_odom_transform_;
        bool joy_flag_=0;
        geometry_msgs::Twist current_twist_;
        nav_msgs::Odometry odom_;
        geometry_msgs::TransformStamped transformStamped_;
         tf2_ros::TransformBroadcaster br_;
        zeus_s2_msgs::joy_control  ps3_control_;

        int driver_count;

        int fdcom;
        int baud_rate_;
        unsigned char buffer_data[100];

        int ret;
        // pthread_t threadid;
        ros::Time now_[4];
        ros::Time last_time_[4];
       
        // short error;

        double wheel_track_4wd_;
        double wheel_track_;
        double encoder_resolution_4wd_;
        double ticks_per_meter_;       //脉冲数/m

        double linear_correction_factor_;
        double angular_correction_factor_;
        double gear_reduction_4wd_;     //减速比
        double wheel_diameter_4wd_;
        double start_rotation_limit_w_;
        double delta_time_[4];
        double delta_xy_ave_;
        double delta_th_;
        double v_linear_;
        double v_angular_;
        double delta_x_;
        double delta_y_;

        double accumulation_x_;
        double accumulation_y_;
        double accumulation_th_;
        double accumulation_left_;
        double accumulation_left_1_;
        double accumulation_right_;
        double accumulation_right_1_;

        double delta_left_;
        double delta_right_;        //里程差值 单位 m
        double delta_left_1_;
        double delta_right_1_;        //里程差值 单位 m
        int32_t rev_left_;
        int32_t rev_left_1_;
        int32_t rev_right_;       
        int32_t rev_right_1_;
        int cur_left_;              //存放脉冲计数值
        int cur_left_1_; 
        int cur_right_;
        int cur_right_1_;
        double encoder_low_wrap_;       //解决编码器溢出
        double encoder_high_wrap_;
        int l_wheel_mult_;              //多圈计数
        int l_wheel_mult_1_;              //多圈计数
        int r_wheel_mult_;
        int r_wheel_mult_1_;
        int encoder_min_;
        int encoder_max_;   

        int32_t line_speed_percent;
        int32_t angle_speed_percent;

      //   int32_t rev_left_;
      //   int32_t rev_right_;
        uint8_t queueFlag;             //用于通信检测标志
        uint8_t can_recv_flag;

        float motor_kp[4];
        float motor_ki[4];
        float motor_kd[4];
        int ele_currVol;
        float speed_l;
        float speed_r;
        short error;


        int16_t  motor_ctlVol_one;
        int16_t  motor_ctlVol_two;
        int16_t  motor_ctlVol_three;
        int16_t  motor_ctlVol_four;
        float left_des;
        float right_des;
        bool  reset_vol;
        bool  mode_selection;
        int     pid_wheel_;
        float  left;
        float   right;

        float  motorEleVol[4]={0};      //驱动器反馈电流值
        int16_t     motorSpeed[4] = {0};       //电机的当前速度
        int16_t     motorTemperature[4] = {0}; //电机温度
        int     driverFaultCode[4] = {0}; //驱动器故障码
        uint8_t  rev_flag;                //接收驱动器信息并处理标志
      //   double   speedTemp;       //用于辅助计算的临时变量
        uint64_t count = 0;                               //根据频率定时判断驱动器通信问题
        float  bias[4] = {0};
        float bias_last[4] = {0};
        float bias_integral[4] = {0};
        uint8_t mode_rev_flag = 0;            //用于判断模式切换是否成功
        bool  mode_isnot = 0;
        uint8_t switching_mode = 0;
        u_int64_t  timeArray[2] = {0};        //用于计时数组
        uint8_t state_temp_  =0;
};

